A number of industrial robotics applications, including automatic robotic assembly, require navigation of a robotic arm to a desired pose with respect to an object or to the environment. This problem is often formulated as a peg-in-hole problem in which a needle-like probe has a single point of contact with an object that can include a hole, and in which the initial pose of the probe with respect to the object is unknown. The pose of the probe with respect to the object has six degrees of freedom (6-DOF), comprising 3-DOF uncertainty in a position of the probe and 3-DOF uncertainty in an orientation of the probe with respect to the object. The initial pose uncertainty may greatly exceed a desired accuracy in all 6-DOF.
Several conventional methods use probing and particle-based Monte-Carlo localization to solve robotic peg-in-hole problems. For example, one method describes a localization using probing and particle filtering for lock and key assembly. The method first densely probes the object with the probe at every (x, y) to obtain a contact position (x, y, z), generating a contact configuration-space map, which describes all possible poses in which the probe has contact with the object. After this preprocessing step of dense probing, the method performs particle filtering by probing the object sequentially with the probe, using the contact positions obtained from the sequential probing as the observations. The observation likelihoods are evaluated using the contact configuration-space map.
Another method similarly describes an exhaustive preprocessing step, densely probing an object with a probe that can have multiple points of contact with the object, and using a force/torque sensor connected to the probe to generate a force-torque map. The force-torque map includes contact force and torque at every possible pose at which there is contact between the probe and the object. This method also describes estimating the force-torque map directly from a computer-aided design (CAD) model. However, the estimated force-torque map is not as accurate as the map acquired by the probing. Using the force-torque map that was acquired in advance by dense probing, particle filtering is used with sequential probing to match the force/torque observations to the map. The method also optionally incorporates observations from a camera.
However, the aforementioned methods are not well suited for full 6-DOF localization, because number of particles required for standard particle filtering increases roughly exponentially with the number of dimensions in the search space. Although those methods are described for uncertainty in up to six degrees of freedom (6-DOF), in practice the methods are only used for localization in lower-dimensional search spaces, such as 2-DOF space in which uncertainty only exists in x, y translation.
Accordingly, it is desired to have a method that can efficiently determine a pose between a probe and an object, wherein the pose has up to six degrees of freedom (6-DOF). It is also desired to have a map representation that can be determined directly from a CAD model of the object without a preprocessing step of dense probing, and that can account for varying levels of uncertainty in the map and for discrepancies between the CAD model and the object.